//
//  point_fit.cpp
//  TetraQuadModel
//
//  Created by Thomas Lewiner on 2/8/12.
//  Copyright 2012. All rights reserved.
//

#include <cmath>

#include "fitting.h"

typedef qreal real;

bool debug = true ;
bool use_normals = false ;
static const bool reduce_higher_coefficients = true;

// weight of normal constraints
static const real C = 0.0001;
// weight of coeff constraints
static const real D = 0.0001;

static const int MAXN = 2046 ;
int N = MAXN ;

// points generated with J = -1, with normalized gradient as normal
//char* filename = (char*)"tetracubJ1Normalized.off";
//static const int N = 137;

// points generated with J != -1, with normalized gradient as normal
//char* filename = (char*)"tetracubNCub.off";
//static const int N = 94;

//real pointset[MAXN][4] ;
//real normals[MAXN][3];
real **pointset ;
real **normals;

real tetra[4][4] = {
  {-1.0,-1.0,-1.0,1.0},  // w0
  {+1.0,+1.0,-1.0,1.0},  // w1
  {+1.0,-1.0,+1.0,1.0},  // w2
  {-1.0,+1.0,+1.0,1.0}   // w3
} ;

int quad_idx[4][4] = {
  {0, 1, 2, 3},
  {1, 4, 5, 6},
  {2, 5, 7, 8},
  {3, 6, 8,-1}
} ;

int idx_quad_j[9] = { 0, 0, 0, 0, 1, 1, 1, 2, 2 } ;
int idx_quad_k[9] = { 0, 1, 2, 3, 1, 2, 3, 2, 3 } ;


real Q[4][4][4] ;
real Q_ref[36] ;



// -------------------------------------------------------- //
// print matrix
void print_vector( const char *name, const int I, const gsl_vector *v )
// -------------------------------------------------------- //
{
  if( !debug ) return ;

  printf( "%s:", name ) ;
  for( int i = 0 ; i < I ; ++i )
  {
    printf( "\t%f", gsl_vector_get( v, i ) ) ;
  }
  printf( "\n" ) ;
}
// -------------------------------------------------------- //



// -------------------------------------------------------- //
// print matrix
void print_matrix( const char *name, const int I, const int J, const gsl_matrix *M )
// -------------------------------------------------------- //
{
  if( !debug ) return ;

  printf( "%s\n", name ) ;
  for( int i = 0 ; i < I ; ++i )
  {
    for( int j = 0 ; j < J ; ++j )
    {
      real x = gsl_matrix_get( M, i,j );
      if( fabs(x) < 1e-6 )
        printf("\t0");
      else
        printf( "\t%f", x ) ;
    }
    printf( "\n" ) ;
  }
  printf( "\n" ) ;
}
// -------------------------------------------------------- //



// -------------------------------------------------------- //
// read off file
bool read_off( const char* pointfilename,  const char* paramfilename )
// -------------------------------------------------------- //
{
        FILE *fp = fopen(pointfilename, "r");
        if( fp == (FILE*)NULL)
  {
    printf("File %s not found\n", pointfilename);
    return false;
  }

        int nv_, nt_;
        if (fscanf(fp, "OFF %d %d 0 ", &nv_, &nt_) != 2) return false;
        nv_ = N ;

        //read vertices and normals
  float x, y, z, nx, ny, nz;
        for( int n = 0 ; n < nv_ ; ++n )
        {

                fscanf( fp, "%f %f %f %f %f %f", &x, &y, &z, &nx, &ny, &nz);
    pointset[n][0] =  x ;
    pointset[n][1] =  y ;
    pointset[n][2] =  z ;
    pointset[n][3] = 1.0 ;
    if( use_normals )
    {
      normals[n][0]  = nx ;
      normals[n][1]  = ny ;
      normals[n][2]  = nz ;
    }
        }

        fclose(fp);


        fp = fopen(paramfilename, "r");
        if( fp == (FILE*)NULL)
  {
    printf("File %s not found\n", paramfilename);
    return false;
  }

        if (fscanf(fp, "tet v0 %lf %lf %lf %lf ", tetra[0]+ 0, tetra[0]+ 1, tetra[0]+ 2, tetra[0]+ 3 ) != 4) return false;
        if (fscanf(fp, "v1 %lf %lf %lf %lf ", tetra[1]+ 0, tetra[1]+ 1, tetra[1]+ 2, tetra[1]+ 3 ) != 4) return false;
        if (fscanf(fp, "v2 %lf %lf %lf %lf ", tetra[2]+ 0, tetra[2]+ 1, tetra[2]+ 2, tetra[2]+ 3 ) != 4) return false;
        if (fscanf(fp, "v3 %lf %lf %lf %lf ", tetra[3]+ 0, tetra[3]+ 1, tetra[3]+ 2, tetra[3]+ 3 ) != 4) return false;

  real j ;
        if (fscanf(fp, "quads q0 %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf ", Q_ref+ 0, Q_ref+ 1, Q_ref+ 2, Q_ref+ 3, Q_ref+ 4, Q_ref+ 5, Q_ref+ 6, Q_ref+ 7, Q_ref+ 8, &j ) != 10) return false;
  if( j != -1.0 ) return false ;
        if (fscanf(fp, "q1 %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf ", Q_ref+ 9, Q_ref+10, Q_ref+11, Q_ref+12, Q_ref+13, Q_ref+14, Q_ref+15, Q_ref+16, Q_ref+17, &j ) != 10) return false;
  if( j != -1.0 ) return false ;
        if (fscanf(fp, "q2 %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf ", Q_ref+18, Q_ref+19, Q_ref+20, Q_ref+21, Q_ref+22, Q_ref+23, Q_ref+24, Q_ref+25, Q_ref+26, &j ) != 10) return false;
  if( j != -1.0 ) return false ;
        if (fscanf(fp, "q3 %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf ", Q_ref+27, Q_ref+28, Q_ref+29, Q_ref+30, Q_ref+31, Q_ref+32, Q_ref+33, Q_ref+34, Q_ref+35, &j ) != 10) return false;
  if( j != -1.0 ) return false ;

        fclose(fp);

  return true;
}
// -------------------------------------------------------- //



// -------------------------------------------------------- //
// generate points
int generate_points()
// -------------------------------------------------------- //
{
  int sqn = floor( sqrt(N) ) ;

  for( int n = 0 ; n < N ; ++n )
  {
    real x = ((n+((real)rand())/RAND_MAX-0.5) / sqn)/sqn - 0.5 ;
    real y = ((real)(n % sqn)+((real)rand())/RAND_MAX-0.5)/sqn - 0.5 ;
    pointset[n][0] =  0.8*sin(4.0*M_PI*x)*sin(4.0*M_PI*y) ;
    pointset[n][1] =  0.2*sin(4.0*M_PI*x)*cos(4.0*M_PI*y) ;
    pointset[n][2] =  0.5*cos(4.0*M_PI*x) ;
    pointset[n][3] = 1.0 ;
    if( use_normals )
    {

    }
  }


  return true ;
}
// -------------------------------------------------------- //




// -------------------------------------------------------- //
// compute barycentric coordinates
int compute_bary_matrix( gsl_matrix *bary )
// -------------------------------------------------------- //
{
  if( !bary ) return false ;

  gsl_matrix *A = gsl_matrix_alloc(4,4);
  for( int j = 0 ; j < 4 ; ++j )
  {
    for( int i = 0 ; i < 4 ; ++i )
    {
      gsl_matrix_set( A, i,j, tetra[j][i] ) ;
    }
  }
  //  print_matrix( "A", 4, 4, A ) ;

  // inverse A
  gsl_permutation *perm  = gsl_permutation_alloc(4);
  int s=0;
  gsl_linalg_LU_decomp(A, perm, &s);
  gsl_linalg_LU_invert(A, perm, bary);
  gsl_permutation_free(perm);
  gsl_matrix_free(A);
  print_matrix( "bary", 4, 4, bary ) ;

  return true ;
}
// -------------------------------------------------------- //



// -------------------------------------------------------- //
// cross product
void cross_product( const real a[], const real b[], real c[] )
// -------------------------------------------------------- //
{
  c[0] = a[1]*b[2] - a[2]*b[1];
  c[1] = a[2]*b[0] - a[0]*b[2];
  c[2] = a[0]*b[1] - a[1]*b[0];
}
// -------------------------------------------------------- //



// -------------------------------------------------------- //
// spectral structure of _m
int eigen(const gsl_matrix *_m)
// -------------------------------------------------------- //
{
  if( !_m ) return false ;

  int n = (int)_m->size1;
  gsl_matrix_view m = gsl_matrix_view_array ( _m->data, n, n );
  gsl_vector *eval  = gsl_vector_alloc ( n );
  gsl_matrix *evec  = gsl_matrix_alloc ( n, n );

  gsl_eigen_symmv_workspace * w = gsl_eigen_symmv_alloc ( n );
  gsl_eigen_symmv ( &m.matrix, eval, evec, w );

  gsl_eigen_symmv_free ( w );

  gsl_eigen_symmv_sort ( eval, evec, GSL_EIGEN_SORT_ABS_ASC );

  {
    int i;
    printf ( "Eigenvalues:\n" );
    for ( i = 0; i < n; i++ )
    {
      double eval_i = gsl_vector_get (eval, i);
      printf ("(%d) %g\t", i, eval_i);
      //      gsl_vector_view evec_i = gsl_matrix_column (evec, i);

      //      printf ("autovalor %d = %g\n", i, eval_i);
      //      printf ("\nautovetor %d = \n", i);
      //
      //      for (int j = 0; j < evec_i.vector.size; ++j)
      //      {
      //        if( j == 36 ) printf("\nLambdas: \n");
      //        real x = gsl_vector_get(&evec_i.vector, j);
      //        if( fabs(x) < 1e-3 ) printf( "0 " ) ; else printf("%g ", x);
      //        //if ( !((j+1)%10) ) printf("\n");
      //      }
      //gsl_vector_fprintf (stdout, &evec_i.vector, "%g");
    }
    printf ("\n\n");
  }

  gsl_vector_free (eval);
  gsl_matrix_free (evec);

  return true ;
}



// -------------------------------------------------------- //
// create matrix of point constraints
int single_point_constraint( const gsl_matrix *bary, gsl_matrix *M, gsl_vector *U, int n, real x, real y, real z, int q0, int q1, int q2, int q3 )
// -------------------------------------------------------- //
{
  gsl_matrix_set( M, n, 9*q0+0,   x * x * gsl_matrix_get( bary, 0, 2 ) * z + x * x * x * gsl_matrix_get( bary, 0, 0 ) + x * x * gsl_matrix_get( bary, 0, 1 ) * y + x * x * gsl_matrix_get( bary, 0, 3 )  ) ;
  gsl_matrix_set( M, n, 9*q0+1,   2.0 * y * x * gsl_matrix_get( bary, 0, 2 ) * z + 2.0 * y * x * gsl_matrix_get( bary, 0, 3 ) + 2.0 * y * y * x * gsl_matrix_get( bary, 0, 1 ) + 2.0 * y * x * x * gsl_matrix_get( bary, 0, 0 )  ) ;
  gsl_matrix_set( M, n, 9*q0+2,   2.0 * z * x * x * gsl_matrix_get( bary, 0, 0 ) + 2.0 * z * x * gsl_matrix_get( bary, 0, 3 ) + 2.0 * z * z * x * gsl_matrix_get( bary, 0, 2 ) + 2.0 * z * x * gsl_matrix_get( bary, 0, 1 ) * y  ) ;
  gsl_matrix_set( M, n, 9*q0+3,   2.0 * x * gsl_matrix_get( bary, 0, 1 ) * y + 2.0 * x * x * gsl_matrix_get( bary, 0, 0 ) + 2.0 * x * gsl_matrix_get( bary, 0, 2 ) * z + 2.0 * x * gsl_matrix_get( bary, 0, 3 )  ) ;
  gsl_matrix_set( M, n, 9*q0+4,   y * y * gsl_matrix_get( bary, 0, 3 ) + y * y * y * gsl_matrix_get( bary, 0, 1 ) + y * y * gsl_matrix_get( bary, 0, 0 ) * x + y * y * gsl_matrix_get( bary, 0, 2 ) * z  ) ;
  gsl_matrix_set( M, n, 9*q0+5,   2.0 * z * y * gsl_matrix_get( bary, 0, 0 ) * x + 2.0 * z * y * gsl_matrix_get( bary, 0, 3 ) + 2.0 * z * y * y * gsl_matrix_get( bary, 0, 1 ) + 2.0 * z * z * y * gsl_matrix_get( bary, 0, 2 )  ) ;
  gsl_matrix_set( M, n, 9*q0+6,   2.0 * y * y * gsl_matrix_get( bary, 0, 1 ) + 2.0 * y * gsl_matrix_get( bary, 0, 0 ) * x + 2.0 * y * gsl_matrix_get( bary, 0, 3 ) + 2.0 * y * gsl_matrix_get( bary, 0, 2 ) * z  ) ;
  gsl_matrix_set( M, n, 9*q0+7,   z * z * gsl_matrix_get( bary, 0, 0 ) * x + z * z * gsl_matrix_get( bary, 0, 1 ) * y + z * z * z * gsl_matrix_get( bary, 0, 2 ) + z * z * gsl_matrix_get( bary, 0, 3 )  ) ;
  gsl_matrix_set( M, n, 9*q0+8,   2.0 * z * gsl_matrix_get( bary, 0, 3 ) + 2.0 * z * gsl_matrix_get( bary, 0, 1 ) * y + 2.0 * z * z * gsl_matrix_get( bary, 0, 2 ) + 2.0 * z * gsl_matrix_get( bary, 0, 0 ) * x  ) ;
  gsl_matrix_set( M, n, 9*q1+0,   x * x * gsl_matrix_get( bary, 1, 3 ) + x * x * gsl_matrix_get( bary, 1, 2 ) * z + x * x * gsl_matrix_get( bary, 1, 1 ) * y + x * x * x * gsl_matrix_get( bary, 1, 0 )  ) ;
  gsl_matrix_set( M, n, 9*q1+1,   2.0 * y * x * gsl_matrix_get( bary, 1, 2 ) * z + 2.0 * y * y * x * gsl_matrix_get( bary, 1, 1 ) + 2.0 * y * x * x * gsl_matrix_get( bary, 1, 0 ) + 2.0 * y * x * gsl_matrix_get( bary, 1, 3 )  ) ;
  gsl_matrix_set( M, n, 9*q1+2,   2.0 * z * x * gsl_matrix_get( bary, 1, 1 ) * y + 2.0 * z * z * x * gsl_matrix_get( bary, 1, 2 ) + 2.0 * z * x * gsl_matrix_get( bary, 1, 3 ) + 2.0 * z * x * x * gsl_matrix_get( bary, 1, 0 )  ) ;
  gsl_matrix_set( M, n, 9*q1+3,   2.0 * x * gsl_matrix_get( bary, 1, 2 ) * z + 2.0 * x * gsl_matrix_get( bary, 1, 3 ) + 2.0 * x * x * gsl_matrix_get( bary, 1, 0 ) + 2.0 * x * gsl_matrix_get( bary, 1, 1 ) * y  ) ;
  gsl_matrix_set( M, n, 9*q1+4,   y * y * y * gsl_matrix_get( bary, 1, 1 ) + y * y * gsl_matrix_get( bary, 1, 0 ) * x + y * y * gsl_matrix_get( bary, 1, 3 ) + y * y * gsl_matrix_get( bary, 1, 2 ) * z  ) ;
  gsl_matrix_set( M, n, 9*q1+5,   2.0 * z * z * y * gsl_matrix_get( bary, 1, 2 ) + 2.0 * z * y * gsl_matrix_get( bary, 1, 0 ) * x + 2.0 * z * y * y * gsl_matrix_get( bary, 1, 1 ) + 2.0 * z * y * gsl_matrix_get( bary, 1, 3 )  ) ;
  gsl_matrix_set( M, n, 9*q1+6,   2.0 * y * gsl_matrix_get( bary, 1, 2 ) * z + 2.0 * y * y * gsl_matrix_get( bary, 1, 1 ) + 2.0 * y * gsl_matrix_get( bary, 1, 3 ) + 2.0 * y * gsl_matrix_get( bary, 1, 0 ) * x  ) ;
  gsl_matrix_set( M, n, 9*q1+7,   z * z * gsl_matrix_get( bary, 1, 0 ) * x + z * z * gsl_matrix_get( bary, 1, 3 ) + z * z * gsl_matrix_get( bary, 1, 1 ) * y + z * z * z * gsl_matrix_get( bary, 1, 2 )  ) ;
  gsl_matrix_set( M, n, 9*q1+8,   2.0 * z * z * gsl_matrix_get( bary, 1, 2 ) + 2.0 * z * gsl_matrix_get( bary, 1, 1 ) * y + 2.0 * z * gsl_matrix_get( bary, 1, 0 ) * x + 2.0 * z * gsl_matrix_get( bary, 1, 3 )  ) ;
  gsl_matrix_set( M, n, 9*q2+0,   x * x * gsl_matrix_get( bary, 2, 3 ) + x * x * gsl_matrix_get( bary, 2, 1 ) * y + x * x * gsl_matrix_get( bary, 2, 2 ) * z + x * x * x * gsl_matrix_get( bary, 2, 0 )  ) ;
  gsl_matrix_set( M, n, 9*q2+1,   2.0 * y * x * gsl_matrix_get( bary, 2, 3 ) + 2.0 * y * x * gsl_matrix_get( bary, 2, 2 ) * z + 2.0 * y * y * x * gsl_matrix_get( bary, 2, 1 ) + 2.0 * y * x * x * gsl_matrix_get( bary, 2, 0 )  ) ;
  gsl_matrix_set( M, n, 9*q2+2,   2.0 * z * x * x * gsl_matrix_get( bary, 2, 0 ) + 2.0 * z * z * x * gsl_matrix_get( bary, 2, 2 ) + 2.0 * z * x * gsl_matrix_get( bary, 2, 1 ) * y + 2.0 * z * x * gsl_matrix_get( bary, 2, 3 )  ) ;
  gsl_matrix_set( M, n, 9*q2+3,   2.0 * x * gsl_matrix_get( bary, 2, 3 ) + 2.0 * x * gsl_matrix_get( bary, 2, 2 ) * z + 2.0 * x * x * gsl_matrix_get( bary, 2, 0 ) + 2.0 * x * gsl_matrix_get( bary, 2, 1 ) * y  ) ;
  gsl_matrix_set( M, n, 9*q2+4,   y * y * gsl_matrix_get( bary, 2, 3 ) + y * y * gsl_matrix_get( bary, 2, 2 ) * z + y * y * gsl_matrix_get( bary, 2, 0 ) * x + y * y * y * gsl_matrix_get( bary, 2, 1 )  ) ;
  gsl_matrix_set( M, n, 9*q2+5,   2.0 * z * y * y * gsl_matrix_get( bary, 2, 1 ) + 2.0 * z * y * gsl_matrix_get( bary, 2, 3 ) + 2.0 * z * y * gsl_matrix_get( bary, 2, 0 ) * x + 2.0 * z * z * y * gsl_matrix_get( bary, 2, 2 )  ) ;
  gsl_matrix_set( M, n, 9*q2+6,   2.0 * y * y * gsl_matrix_get( bary, 2, 1 ) + 2.0 * y * gsl_matrix_get( bary, 2, 3 ) + 2.0 * y * gsl_matrix_get( bary, 2, 2 ) * z + 2.0 * y * gsl_matrix_get( bary, 2, 0 ) * x  ) ;
  gsl_matrix_set( M, n, 9*q2+7,   z * z * z * gsl_matrix_get( bary, 2, 2 ) + z * z * gsl_matrix_get( bary, 2, 1 ) * y + z * z * gsl_matrix_get( bary, 2, 0 ) * x + z * z * gsl_matrix_get( bary, 2, 3 )  ) ;
  gsl_matrix_set( M, n, 9*q2+8,   2.0 * z * gsl_matrix_get( bary, 2, 3 ) + 2.0 * z * gsl_matrix_get( bary, 2, 1 ) * y + 2.0 * z * z * gsl_matrix_get( bary, 2, 2 ) + 2.0 * z * gsl_matrix_get( bary, 2, 0 ) * x  ) ;
  gsl_matrix_set( M, n, 9*q3+0,   x * x * x * gsl_matrix_get( bary, 3, 0 ) + x * x * gsl_matrix_get( bary, 3, 2 ) * z + x * x * gsl_matrix_get( bary, 3, 3 ) + x * x * gsl_matrix_get( bary, 3, 1 ) * y  ) ;
  gsl_matrix_set( M, n, 9*q3+1,   2.0 * y * y * x * gsl_matrix_get( bary, 3, 1 ) + 2.0 * y * x * gsl_matrix_get( bary, 3, 3 ) + 2.0 * y * x * x * gsl_matrix_get( bary, 3, 0 ) + 2.0 * y * x * gsl_matrix_get( bary, 3, 2 ) * z  ) ;
  gsl_matrix_set( M, n, 9*q3+2,   2.0 * z * x * gsl_matrix_get( bary, 3, 3 ) + 2.0 * z * z * x * gsl_matrix_get( bary, 3, 2 ) + 2.0 * z * x * gsl_matrix_get( bary, 3, 1 ) * y + 2.0 * z * x * x * gsl_matrix_get( bary, 3, 0 )  ) ;
  gsl_matrix_set( M, n, 9*q3+3,   2.0 * x * gsl_matrix_get( bary, 3, 1 ) * y + 2.0 * x * gsl_matrix_get( bary, 3, 2 ) * z + 2.0 * x * gsl_matrix_get( bary, 3, 3 ) + 2.0 * x * x * gsl_matrix_get( bary, 3, 0 )  ) ;
  gsl_matrix_set( M, n, 9*q3+4,   y * y * gsl_matrix_get( bary, 3, 3 ) + y * y * gsl_matrix_get( bary, 3, 2 ) * z + y * y * gsl_matrix_get( bary, 3, 0 ) * x + y * y * y * gsl_matrix_get( bary, 3, 1 )  ) ;
  gsl_matrix_set( M, n, 9*q3+5,   2.0 * z * y * y * gsl_matrix_get( bary, 3, 1 ) + 2.0 * z * y * gsl_matrix_get( bary, 3, 0 ) * x + 2.0 * z * y * gsl_matrix_get( bary, 3, 3 ) + 2.0 * z * z * y * gsl_matrix_get( bary, 3, 2 )  ) ;
  gsl_matrix_set( M, n, 9*q3+6,   2.0 * y * gsl_matrix_get( bary, 3, 2 ) * z + 2.0 * y * y * gsl_matrix_get( bary, 3, 1 ) + 2.0 * y * gsl_matrix_get( bary, 3, 3 ) + 2.0 * y * gsl_matrix_get( bary, 3, 0 ) * x  ) ;
  gsl_matrix_set( M, n, 9*q3+7,   z * z * gsl_matrix_get( bary, 3, 1 ) * y + z * z * z * gsl_matrix_get( bary, 3, 2 ) + z * z * gsl_matrix_get( bary, 3, 0 ) * x + z * z * gsl_matrix_get( bary, 3, 3 )  ) ;
  gsl_matrix_set( M, n, 9*q3+8,   2.0 * z * gsl_matrix_get( bary, 3, 1 ) * y + 2.0 * z * gsl_matrix_get( bary, 3, 3 ) + 2.0 * z * z * gsl_matrix_get( bary, 3, 2 ) + 2.0 * z * gsl_matrix_get( bary, 3, 0 ) * x  ) ;

  gsl_vector_set( U, n,   gsl_matrix_get( bary, 3, 3 ) + gsl_matrix_get( bary, 0, 3 ) + gsl_matrix_get( bary, 1, 3 ) + gsl_matrix_get( bary, 2, 3 ) + gsl_matrix_get( bary, 0, 0 ) * x + gsl_matrix_get( bary, 0, 1 ) * y + gsl_matrix_get( bary, 0, 2 ) * z + gsl_matrix_get( bary, 1, 0 ) * x + gsl_matrix_get( bary, 1, 1 ) * y + gsl_matrix_get( bary, 1, 2 ) * z + gsl_matrix_get( bary, 2, 0 ) * x + gsl_matrix_get( bary, 2, 1 ) * y + gsl_matrix_get( bary, 2, 2 ) * z + gsl_matrix_get( bary, 3, 0 ) * x + gsl_matrix_get( bary, 3, 1 ) * y + gsl_matrix_get( bary, 3, 2 ) * z  ) ;

  return true ;
}



// -------------------------------------------------------- //
// create matrix of point and normal constraints
int single_point_normal_constraint( const gsl_matrix *bary, gsl_matrix *M, gsl_vector *U, int n, real x, real y, real z, real nx, real ny, real nz, int q0, int q1, int q2, int q3 )
// -------------------------------------------------------- //
{
  gsl_matrix_set( M, 4*n+0, 9*q0+0,   x * x * gsl_matrix_get( bary, 0, 2 ) * z + x * x * x * gsl_matrix_get( bary, 0, 0 ) + x * x * gsl_matrix_get( bary, 0, 1 ) * y + x * x * gsl_matrix_get( bary, 0, 3 )  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q0+1,   2.0 * y * x * gsl_matrix_get( bary, 0, 2 ) * z + 2.0 * y * x * gsl_matrix_get( bary, 0, 3 ) + 2.0 * y * y * x * gsl_matrix_get( bary, 0, 1 ) + 2.0 * y * x * x * gsl_matrix_get( bary, 0, 0 )  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q0+2,   2.0 * z * x * x * gsl_matrix_get( bary, 0, 0 ) + 2.0 * z * x * gsl_matrix_get( bary, 0, 3 ) + 2.0 * z * z * x * gsl_matrix_get( bary, 0, 2 ) + 2.0 * z * x * gsl_matrix_get( bary, 0, 1 ) * y  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q0+3,   2.0 * x * gsl_matrix_get( bary, 0, 1 ) * y + 2.0 * x * x * gsl_matrix_get( bary, 0, 0 ) + 2.0 * x * gsl_matrix_get( bary, 0, 2 ) * z + 2.0 * x * gsl_matrix_get( bary, 0, 3 )  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q0+4,   y * y * gsl_matrix_get( bary, 0, 3 ) + y * y * y * gsl_matrix_get( bary, 0, 1 ) + y * y * gsl_matrix_get( bary, 0, 0 ) * x + y * y * gsl_matrix_get( bary, 0, 2 ) * z  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q0+5,   2.0 * z * y * gsl_matrix_get( bary, 0, 0 ) * x + 2.0 * z * y * gsl_matrix_get( bary, 0, 3 ) + 2.0 * z * y * y * gsl_matrix_get( bary, 0, 1 ) + 2.0 * z * z * y * gsl_matrix_get( bary, 0, 2 )  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q0+6,   2.0 * y * y * gsl_matrix_get( bary, 0, 1 ) + 2.0 * y * gsl_matrix_get( bary, 0, 0 ) * x + 2.0 * y * gsl_matrix_get( bary, 0, 3 ) + 2.0 * y * gsl_matrix_get( bary, 0, 2 ) * z  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q0+7,   z * z * gsl_matrix_get( bary, 0, 0 ) * x + z * z * gsl_matrix_get( bary, 0, 1 ) * y + z * z * z * gsl_matrix_get( bary, 0, 2 ) + z * z * gsl_matrix_get( bary, 0, 3 )  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q0+8,   2.0 * z * gsl_matrix_get( bary, 0, 3 ) + 2.0 * z * gsl_matrix_get( bary, 0, 1 ) * y + 2.0 * z * z * gsl_matrix_get( bary, 0, 2 ) + 2.0 * z * gsl_matrix_get( bary, 0, 0 ) * x  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q1+0,   x * x * gsl_matrix_get( bary, 1, 3 ) + x * x * gsl_matrix_get( bary, 1, 2 ) * z + x * x * gsl_matrix_get( bary, 1, 1 ) * y + x * x * x * gsl_matrix_get( bary, 1, 0 )  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q1+1,   2.0 * y * x * gsl_matrix_get( bary, 1, 2 ) * z + 2.0 * y * y * x * gsl_matrix_get( bary, 1, 1 ) + 2.0 * y * x * x * gsl_matrix_get( bary, 1, 0 ) + 2.0 * y * x * gsl_matrix_get( bary, 1, 3 )  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q1+2,   2.0 * z * x * gsl_matrix_get( bary, 1, 1 ) * y + 2.0 * z * z * x * gsl_matrix_get( bary, 1, 2 ) + 2.0 * z * x * gsl_matrix_get( bary, 1, 3 ) + 2.0 * z * x * x * gsl_matrix_get( bary, 1, 0 )  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q1+3,   2.0 * x * gsl_matrix_get( bary, 1, 2 ) * z + 2.0 * x * gsl_matrix_get( bary, 1, 3 ) + 2.0 * x * x * gsl_matrix_get( bary, 1, 0 ) + 2.0 * x * gsl_matrix_get( bary, 1, 1 ) * y  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q1+4,   y * y * y * gsl_matrix_get( bary, 1, 1 ) + y * y * gsl_matrix_get( bary, 1, 0 ) * x + y * y * gsl_matrix_get( bary, 1, 3 ) + y * y * gsl_matrix_get( bary, 1, 2 ) * z  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q1+5,   2.0 * z * z * y * gsl_matrix_get( bary, 1, 2 ) + 2.0 * z * y * gsl_matrix_get( bary, 1, 0 ) * x + 2.0 * z * y * y * gsl_matrix_get( bary, 1, 1 ) + 2.0 * z * y * gsl_matrix_get( bary, 1, 3 )  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q1+6,   2.0 * y * gsl_matrix_get( bary, 1, 2 ) * z + 2.0 * y * y * gsl_matrix_get( bary, 1, 1 ) + 2.0 * y * gsl_matrix_get( bary, 1, 3 ) + 2.0 * y * gsl_matrix_get( bary, 1, 0 ) * x  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q1+7,   z * z * gsl_matrix_get( bary, 1, 0 ) * x + z * z * gsl_matrix_get( bary, 1, 3 ) + z * z * gsl_matrix_get( bary, 1, 1 ) * y + z * z * z * gsl_matrix_get( bary, 1, 2 )  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q1+8,   2.0 * z * z * gsl_matrix_get( bary, 1, 2 ) + 2.0 * z * gsl_matrix_get( bary, 1, 1 ) * y + 2.0 * z * gsl_matrix_get( bary, 1, 0 ) * x + 2.0 * z * gsl_matrix_get( bary, 1, 3 )  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q2+0,   x * x * gsl_matrix_get( bary, 2, 3 ) + x * x * gsl_matrix_get( bary, 2, 1 ) * y + x * x * gsl_matrix_get( bary, 2, 2 ) * z + x * x * x * gsl_matrix_get( bary, 2, 0 )  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q2+1,   2.0 * y * x * gsl_matrix_get( bary, 2, 3 ) + 2.0 * y * x * gsl_matrix_get( bary, 2, 2 ) * z + 2.0 * y * y * x * gsl_matrix_get( bary, 2, 1 ) + 2.0 * y * x * x * gsl_matrix_get( bary, 2, 0 )  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q2+2,   2.0 * z * x * x * gsl_matrix_get( bary, 2, 0 ) + 2.0 * z * z * x * gsl_matrix_get( bary, 2, 2 ) + 2.0 * z * x * gsl_matrix_get( bary, 2, 1 ) * y + 2.0 * z * x * gsl_matrix_get( bary, 2, 3 )  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q2+3,   2.0 * x * gsl_matrix_get( bary, 2, 3 ) + 2.0 * x * gsl_matrix_get( bary, 2, 2 ) * z + 2.0 * x * x * gsl_matrix_get( bary, 2, 0 ) + 2.0 * x * gsl_matrix_get( bary, 2, 1 ) * y  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q2+4,   y * y * gsl_matrix_get( bary, 2, 3 ) + y * y * gsl_matrix_get( bary, 2, 2 ) * z + y * y * gsl_matrix_get( bary, 2, 0 ) * x + y * y * y * gsl_matrix_get( bary, 2, 1 )  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q2+5,   2.0 * z * y * y * gsl_matrix_get( bary, 2, 1 ) + 2.0 * z * y * gsl_matrix_get( bary, 2, 3 ) + 2.0 * z * y * gsl_matrix_get( bary, 2, 0 ) * x + 2.0 * z * z * y * gsl_matrix_get( bary, 2, 2 )  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q2+6,   2.0 * y * y * gsl_matrix_get( bary, 2, 1 ) + 2.0 * y * gsl_matrix_get( bary, 2, 3 ) + 2.0 * y * gsl_matrix_get( bary, 2, 2 ) * z + 2.0 * y * gsl_matrix_get( bary, 2, 0 ) * x  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q2+7,   z * z * z * gsl_matrix_get( bary, 2, 2 ) + z * z * gsl_matrix_get( bary, 2, 1 ) * y + z * z * gsl_matrix_get( bary, 2, 0 ) * x + z * z * gsl_matrix_get( bary, 2, 3 )  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q2+8,   2.0 * z * gsl_matrix_get( bary, 2, 3 ) + 2.0 * z * gsl_matrix_get( bary, 2, 1 ) * y + 2.0 * z * z * gsl_matrix_get( bary, 2, 2 ) + 2.0 * z * gsl_matrix_get( bary, 2, 0 ) * x  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q3+0,   x * x * x * gsl_matrix_get( bary, 3, 0 ) + x * x * gsl_matrix_get( bary, 3, 2 ) * z + x * x * gsl_matrix_get( bary, 3, 3 ) + x * x * gsl_matrix_get( bary, 3, 1 ) * y  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q3+1,   2.0 * y * y * x * gsl_matrix_get( bary, 3, 1 ) + 2.0 * y * x * gsl_matrix_get( bary, 3, 3 ) + 2.0 * y * x * x * gsl_matrix_get( bary, 3, 0 ) + 2.0 * y * x * gsl_matrix_get( bary, 3, 2 ) * z  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q3+2,   2.0 * z * x * gsl_matrix_get( bary, 3, 3 ) + 2.0 * z * z * x * gsl_matrix_get( bary, 3, 2 ) + 2.0 * z * x * gsl_matrix_get( bary, 3, 1 ) * y + 2.0 * z * x * x * gsl_matrix_get( bary, 3, 0 )  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q3+3,   2.0 * x * gsl_matrix_get( bary, 3, 1 ) * y + 2.0 * x * gsl_matrix_get( bary, 3, 2 ) * z + 2.0 * x * gsl_matrix_get( bary, 3, 3 ) + 2.0 * x * x * gsl_matrix_get( bary, 3, 0 )  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q3+4,   y * y * gsl_matrix_get( bary, 3, 3 ) + y * y * gsl_matrix_get( bary, 3, 2 ) * z + y * y * gsl_matrix_get( bary, 3, 0 ) * x + y * y * y * gsl_matrix_get( bary, 3, 1 )  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q3+5,   2.0 * z * y * y * gsl_matrix_get( bary, 3, 1 ) + 2.0 * z * y * gsl_matrix_get( bary, 3, 0 ) * x + 2.0 * z * y * gsl_matrix_get( bary, 3, 3 ) + 2.0 * z * z * y * gsl_matrix_get( bary, 3, 2 )  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q3+6,   2.0 * y * gsl_matrix_get( bary, 3, 2 ) * z + 2.0 * y * y * gsl_matrix_get( bary, 3, 1 ) + 2.0 * y * gsl_matrix_get( bary, 3, 3 ) + 2.0 * y * gsl_matrix_get( bary, 3, 0 ) * x  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q3+7,   z * z * gsl_matrix_get( bary, 3, 1 ) * y + z * z * z * gsl_matrix_get( bary, 3, 2 ) + z * z * gsl_matrix_get( bary, 3, 0 ) * x + z * z * gsl_matrix_get( bary, 3, 3 )  ) ;
  gsl_matrix_set( M, 4*n+0, 9*q3+8,   2.0 * z * gsl_matrix_get( bary, 3, 1 ) * y + 2.0 * z * gsl_matrix_get( bary, 3, 3 ) + 2.0 * z * z * gsl_matrix_get( bary, 3, 2 ) + 2.0 * z * gsl_matrix_get( bary, 3, 0 ) * x  ) ;
  gsl_matrix_set( M, 4*n+1, 9*q0+0,   C * (-gsl_matrix_get( bary, 0, 2 ) * x * x * ny + gsl_matrix_get( bary, 0, 1 ) * x * x * nz  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q0+1,   C * (-2.0 * gsl_matrix_get( bary, 0, 2 ) * y * x * ny + ((2.0 * gsl_matrix_get( bary, 0, 3 ) + 4.0 * gsl_matrix_get( bary, 0, 1 ) * y + 2.0 * gsl_matrix_get( bary, 0, 2 ) * z) * x + 2.0 * x * x * gsl_matrix_get( bary, 0, 0 )) * nz  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q0+2,   C * (2.0 * gsl_matrix_get( bary, 0, 1 ) * z * x * nz - ((2.0 * gsl_matrix_get( bary, 0, 3 ) + 4.0 * gsl_matrix_get( bary, 0, 2 ) * z + 2.0 * gsl_matrix_get( bary, 0, 1 ) * y) * x + 2.0 * x * x * gsl_matrix_get( bary, 0, 0 )) * ny  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q0+3,   C * (2.0 * gsl_matrix_get( bary, 0, 1 ) * x * nz - 2.0 * gsl_matrix_get( bary, 0, 2 ) * x * ny  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q0+4,   C * (((2.0 * gsl_matrix_get( bary, 0, 2 ) * z + 2.0 * gsl_matrix_get( bary, 0, 3 )) * y + 3.0 * y * y * gsl_matrix_get( bary, 0, 1 ) + 2.0 * y * gsl_matrix_get( bary, 0, 0 ) * x) * nz - gsl_matrix_get( bary, 0, 2 ) * y * y * ny  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q0+5,   C * ((2.0 * z * gsl_matrix_get( bary, 0, 3 ) + 2.0 * z * z * gsl_matrix_get( bary, 0, 2 ) + 4.0 * z * gsl_matrix_get( bary, 0, 1 ) * y + 2.0 * z * gsl_matrix_get( bary, 0, 0 ) * x) * nz - ((2.0 * gsl_matrix_get( bary, 0, 3 ) + 4.0 * gsl_matrix_get( bary, 0, 2 ) * z) * y + 2.0 * y * y * gsl_matrix_get( bary, 0, 1 ) + 2.0 * y * gsl_matrix_get( bary, 0, 0 ) * x) * ny  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q0+6,   C * ((2.0 * gsl_matrix_get( bary, 0, 3 ) + 4.0 * gsl_matrix_get( bary, 0, 1 ) * y + 2.0 * gsl_matrix_get( bary, 0, 2 ) * z + 2.0 * gsl_matrix_get( bary, 0, 0 ) * x) * nz - 2.0 * gsl_matrix_get( bary, 0, 2 ) * y * ny  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q0+7,   C * (gsl_matrix_get( bary, 0, 1 ) * z * z * nz - (2.0 * z * gsl_matrix_get( bary, 0, 1 ) * y + 2.0 * z * gsl_matrix_get( bary, 0, 3 ) + 3.0 * z * z * gsl_matrix_get( bary, 0, 2 ) + 2.0 * z * gsl_matrix_get( bary, 0, 0 ) * x) * ny  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q0+8,   C * (2.0 * gsl_matrix_get( bary, 0, 1 ) * z * nz - (2.0 * gsl_matrix_get( bary, 0, 3 ) + 2.0 * gsl_matrix_get( bary, 0, 1 ) * y + 4.0 * gsl_matrix_get( bary, 0, 2 ) * z + 2.0 * gsl_matrix_get( bary, 0, 0 ) * x) * ny  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q1+0,   C * (gsl_matrix_get( bary, 1, 1 ) * x * x * nz - gsl_matrix_get( bary, 1, 2 ) * x * x * ny  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q1+1,   C * (((4.0 * gsl_matrix_get( bary, 1, 1 ) * y + 2.0 * gsl_matrix_get( bary, 1, 2 ) * z + 2.0 * gsl_matrix_get( bary, 1, 3 )) * x + 2.0 * x * x * gsl_matrix_get( bary, 1, 0 )) * nz - 2.0 * gsl_matrix_get( bary, 1, 2 ) * y * x * ny  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q1+2,   C * (2.0 * gsl_matrix_get( bary, 1, 1 ) * z * x * nz - ((2.0 * gsl_matrix_get( bary, 1, 1 ) * y + 2.0 * gsl_matrix_get( bary, 1, 3 ) + 4.0 * gsl_matrix_get( bary, 1, 2 ) * z) * x + 2.0 * x * x * gsl_matrix_get( bary, 1, 0 )) * ny  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q1+3,   C * (2.0 * gsl_matrix_get( bary, 1, 1 ) * x * nz - 2.0 * gsl_matrix_get( bary, 1, 2 ) * x * ny  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q1+4,   C * (((2.0 * gsl_matrix_get( bary, 1, 2 ) * z + 2.0 * gsl_matrix_get( bary, 1, 3 )) * y + 2.0 * y * gsl_matrix_get( bary, 1, 0 ) * x + 3.0 * y * y * gsl_matrix_get( bary, 1, 1 )) * nz - gsl_matrix_get( bary, 1, 2 ) * y * y * ny  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q1+5,   C * ((4.0 * z * gsl_matrix_get( bary, 1, 1 ) * y + 2.0 * z * gsl_matrix_get( bary, 1, 3 ) + 2.0 * z * gsl_matrix_get( bary, 1, 0 ) * x + 2.0 * z * z * gsl_matrix_get( bary, 1, 2 )) * nz - (2.0 * y * y * gsl_matrix_get( bary, 1, 1 ) + 2.0 * y * gsl_matrix_get( bary, 1, 0 ) * x + (4.0 * gsl_matrix_get( bary, 1, 2 ) * z + 2.0 * gsl_matrix_get( bary, 1, 3 )) * y) * ny  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q1+6,   C * ((2.0 * gsl_matrix_get( bary, 1, 0 ) * x + 2.0 * gsl_matrix_get( bary, 1, 3 ) + 4.0 * gsl_matrix_get( bary, 1, 1 ) * y + 2.0 * gsl_matrix_get( bary, 1, 2 ) * z) * nz - 2.0 * gsl_matrix_get( bary, 1, 2 ) * y * ny  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q1+7,   C * (gsl_matrix_get( bary, 1, 1 ) * z * z * nz - (2.0 * z * gsl_matrix_get( bary, 1, 0 ) * x + 2.0 * z * gsl_matrix_get( bary, 1, 3 ) + 2.0 * z * gsl_matrix_get( bary, 1, 1 ) * y + 3.0 * z * z * gsl_matrix_get( bary, 1, 2 )) * ny  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q1+8,   C * (2.0 * gsl_matrix_get( bary, 1, 1 ) * z * nz - (2.0 * gsl_matrix_get( bary, 1, 0 ) * x + 2.0 * gsl_matrix_get( bary, 1, 3 ) + 2.0 * gsl_matrix_get( bary, 1, 1 ) * y + 4.0 * gsl_matrix_get( bary, 1, 2 ) * z) * ny  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q2+0,   C * (gsl_matrix_get( bary, 2, 1 ) * x * x * nz - gsl_matrix_get( bary, 2, 2 ) * x * x * ny  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q2+1,   C * (((2.0 * gsl_matrix_get( bary, 2, 2 ) * z + 2.0 * gsl_matrix_get( bary, 2, 3 ) + 4.0 * gsl_matrix_get( bary, 2, 1 ) * y) * x + 2.0 * x * x * gsl_matrix_get( bary, 2, 0 )) * nz - 2.0 * gsl_matrix_get( bary, 2, 2 ) * y * x * ny  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q2+2,   C * (2.0 * gsl_matrix_get( bary, 2, 1 ) * z * x * nz - (2.0 * x * x * gsl_matrix_get( bary, 2, 0 ) + (2.0 * gsl_matrix_get( bary, 2, 3 ) + 4.0 * gsl_matrix_get( bary, 2, 2 ) * z + 2.0 * gsl_matrix_get( bary, 2, 1 ) * y) * x) * ny  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q2+3,   C * (2.0 * gsl_matrix_get( bary, 2, 1 ) * x * nz - 2.0 * gsl_matrix_get( bary, 2, 2 ) * x * ny  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q2+4,   C * (((2.0 * gsl_matrix_get( bary, 2, 3 ) + 2.0 * gsl_matrix_get( bary, 2, 2 ) * z) * y + 3.0 * y * y * gsl_matrix_get( bary, 2, 1 ) + 2.0 * y * gsl_matrix_get( bary, 2, 0 ) * x) * nz - gsl_matrix_get( bary, 2, 2 ) * y * y * ny  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q2+5,   C * ((2.0 * z * z * gsl_matrix_get( bary, 2, 2 ) + 4.0 * z * gsl_matrix_get( bary, 2, 1 ) * y + 2.0 * z * gsl_matrix_get( bary, 2, 3 ) + 2.0 * z * gsl_matrix_get( bary, 2, 0 ) * x) * nz - (2.0 * y * y * gsl_matrix_get( bary, 2, 1 ) + 2.0 * y * gsl_matrix_get( bary, 2, 0 ) * x + (4.0 * gsl_matrix_get( bary, 2, 2 ) * z + 2.0 * gsl_matrix_get( bary, 2, 3 )) * y) * ny  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q2+6,   C * ((2.0 * gsl_matrix_get( bary, 2, 0 ) * x + 2.0 * gsl_matrix_get( bary, 2, 3 ) + 4.0 * gsl_matrix_get( bary, 2, 1 ) * y + 2.0 * gsl_matrix_get( bary, 2, 2 ) * z) * nz - 2.0 * gsl_matrix_get( bary, 2, 2 ) * y * ny  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q2+7,   C * (gsl_matrix_get( bary, 2, 1 ) * z * z * nz - (3.0 * z * z * gsl_matrix_get( bary, 2, 2 ) + 2.0 * z * gsl_matrix_get( bary, 2, 3 ) + 2.0 * z * gsl_matrix_get( bary, 2, 0 ) * x + 2.0 * z * gsl_matrix_get( bary, 2, 1 ) * y) * ny  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q2+8,   C * (2.0 * gsl_matrix_get( bary, 2, 1 ) * z * nz - (2.0 * gsl_matrix_get( bary, 2, 3 ) + 4.0 * gsl_matrix_get( bary, 2, 2 ) * z + 2.0 * gsl_matrix_get( bary, 2, 0 ) * x + 2.0 * gsl_matrix_get( bary, 2, 1 ) * y) * ny  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q3+0,   C * (gsl_matrix_get( bary, 3, 1 ) * x * x * nz - gsl_matrix_get( bary, 3, 2 ) * x * x * ny  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q3+1,   C * ((2.0 * x * x * gsl_matrix_get( bary, 3, 0 ) + (4.0 * gsl_matrix_get( bary, 3, 1 ) * y + 2.0 * gsl_matrix_get( bary, 3, 2 ) * z + 2.0 * gsl_matrix_get( bary, 3, 3 )) * x) * nz - 2.0 * gsl_matrix_get( bary, 3, 2 ) * y * x * ny  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q3+2,   C * (2.0 * gsl_matrix_get( bary, 3, 1 ) * z * x * nz - (2.0 * x * x * gsl_matrix_get( bary, 3, 0 ) + (2.0 * gsl_matrix_get( bary, 3, 1 ) * y + 2.0 * gsl_matrix_get( bary, 3, 3 ) + 4.0 * gsl_matrix_get( bary, 3, 2 ) * z) * x) * ny  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q3+3,   C * (2.0 * gsl_matrix_get( bary, 3, 1 ) * x * nz - 2.0 * gsl_matrix_get( bary, 3, 2 ) * x * ny  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q3+4,   C * ((2.0 * y * gsl_matrix_get( bary, 3, 0 ) * x + 3.0 * y * y * gsl_matrix_get( bary, 3, 1 ) + (2.0 * gsl_matrix_get( bary, 3, 2 ) * z + 2.0 * gsl_matrix_get( bary, 3, 3 )) * y) * nz - gsl_matrix_get( bary, 3, 2 ) * y * y * ny  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q3+5,   C * ((2.0 * z * gsl_matrix_get( bary, 3, 3 ) + 2.0 * z * z * gsl_matrix_get( bary, 3, 2 ) + 2.0 * z * gsl_matrix_get( bary, 3, 0 ) * x + 4.0 * z * gsl_matrix_get( bary, 3, 1 ) * y) * nz - (2.0 * y * gsl_matrix_get( bary, 3, 0 ) * x + 2.0 * y * y * gsl_matrix_get( bary, 3, 1 ) + (4.0 * gsl_matrix_get( bary, 3, 2 ) * z + 2.0 * gsl_matrix_get( bary, 3, 3 )) * y) * ny  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q3+6,   C * ((2.0 * gsl_matrix_get( bary, 3, 0 ) * x + 2.0 * gsl_matrix_get( bary, 3, 3 ) + 4.0 * gsl_matrix_get( bary, 3, 1 ) * y + 2.0 * gsl_matrix_get( bary, 3, 2 ) * z) * nz - 2.0 * gsl_matrix_get( bary, 3, 2 ) * y * ny  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q3+7,   C * (z * z * gsl_matrix_get( bary, 3, 1 ) * nz - (3.0 * z * z * gsl_matrix_get( bary, 3, 2 ) + 2.0 * z * gsl_matrix_get( bary, 3, 1 ) * y + 2.0 * z * gsl_matrix_get( bary, 3, 3 ) + 2.0 * z * gsl_matrix_get( bary, 3, 0 ) * x) * ny  )   ) ;
  gsl_matrix_set( M, 4*n+1, 9*q3+8,   C * (2.0 * gsl_matrix_get( bary, 3, 1 ) * z * nz - (2.0 * gsl_matrix_get( bary, 3, 1 ) * y + 2.0 * gsl_matrix_get( bary, 3, 3 ) + 2.0 * gsl_matrix_get( bary, 3, 0 ) * x + 4.0 * gsl_matrix_get( bary, 3, 2 ) * z) * ny  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q0+0,   C * (-(3.0 * x * x * gsl_matrix_get( bary, 0, 0 ) + (2.0 * gsl_matrix_get( bary, 0, 3 ) + 2.0 * gsl_matrix_get( bary, 0, 1 ) * y + 2.0 * gsl_matrix_get( bary, 0, 2 ) * z) * x) * nz + gsl_matrix_get( bary, 0, 2 ) * x * x * nx  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q0+1,   C * (-((2.0 * gsl_matrix_get( bary, 0, 2 ) * z + 2.0 * gsl_matrix_get( bary, 0, 3 )) * y + 2.0 * y * y * gsl_matrix_get( bary, 0, 1 ) + 4.0 * y * gsl_matrix_get( bary, 0, 0 ) * x) * nz + 2.0 * gsl_matrix_get( bary, 0, 2 ) * y * x * nx  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q0+2,   C * (((2.0 * gsl_matrix_get( bary, 0, 3 ) + 4.0 * gsl_matrix_get( bary, 0, 2 ) * z + 2.0 * gsl_matrix_get( bary, 0, 1 ) * y) * x + 2.0 * x * x * gsl_matrix_get( bary, 0, 0 )) * nx - (2.0 * z * gsl_matrix_get( bary, 0, 3 ) + 2.0 * z * gsl_matrix_get( bary, 0, 1 ) * y + 4.0 * z * gsl_matrix_get( bary, 0, 0 ) * x + 2.0 * z * z * gsl_matrix_get( bary, 0, 2 )) * nz  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q0+3,   C * (2.0 * gsl_matrix_get( bary, 0, 2 ) * x * nx - (2.0 * gsl_matrix_get( bary, 0, 2 ) * z + 2.0 * gsl_matrix_get( bary, 0, 1 ) * y + 2.0 * gsl_matrix_get( bary, 0, 3 ) + 4.0 * gsl_matrix_get( bary, 0, 0 ) * x) * nz  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q0+4,   C * (gsl_matrix_get( bary, 0, 2 ) * y * y * nx - gsl_matrix_get( bary, 0, 0 ) * y * y * nz  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q0+5,   C * (((2.0 * gsl_matrix_get( bary, 0, 3 ) + 4.0 * gsl_matrix_get( bary, 0, 2 ) * z) * y + 2.0 * y * y * gsl_matrix_get( bary, 0, 1 ) + 2.0 * y * gsl_matrix_get( bary, 0, 0 ) * x) * nx - 2.0 * gsl_matrix_get( bary, 0, 0 ) * z * y * nz  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q0+6,   C * (2.0 * gsl_matrix_get( bary, 0, 2 ) * y * nx - 2.0 * gsl_matrix_get( bary, 0, 0 ) * y * nz  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q0+7,   C * ((2.0 * z * gsl_matrix_get( bary, 0, 1 ) * y + 2.0 * z * gsl_matrix_get( bary, 0, 3 ) + 3.0 * z * z * gsl_matrix_get( bary, 0, 2 ) + 2.0 * z * gsl_matrix_get( bary, 0, 0 ) * x) * nx - gsl_matrix_get( bary, 0, 0 ) * z * z * nz  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q0+8,   C * ((2.0 * gsl_matrix_get( bary, 0, 3 ) + 2.0 * gsl_matrix_get( bary, 0, 1 ) * y + 4.0 * gsl_matrix_get( bary, 0, 2 ) * z + 2.0 * gsl_matrix_get( bary, 0, 0 ) * x) * nx - 2.0 * gsl_matrix_get( bary, 0, 0 ) * z * nz  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q1+0,   C * (gsl_matrix_get( bary, 1, 2 ) * x * x * nx - ((2.0 * gsl_matrix_get( bary, 1, 1 ) * y + 2.0 * gsl_matrix_get( bary, 1, 3 ) + 2.0 * gsl_matrix_get( bary, 1, 2 ) * z) * x + 3.0 * x * x * gsl_matrix_get( bary, 1, 0 )) * nz  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q1+1,   C * (2.0 * gsl_matrix_get( bary, 1, 2 ) * y * x * nx - (4.0 * y * gsl_matrix_get( bary, 1, 0 ) * x + (2.0 * gsl_matrix_get( bary, 1, 2 ) * z + 2.0 * gsl_matrix_get( bary, 1, 3 )) * y + 2.0 * y * y * gsl_matrix_get( bary, 1, 1 )) * nz  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q1+2,   C * (((2.0 * gsl_matrix_get( bary, 1, 1 ) * y + 2.0 * gsl_matrix_get( bary, 1, 3 ) + 4.0 * gsl_matrix_get( bary, 1, 2 ) * z) * x + 2.0 * x * x * gsl_matrix_get( bary, 1, 0 )) * nx - (4.0 * z * gsl_matrix_get( bary, 1, 0 ) * x + 2.0 * z * gsl_matrix_get( bary, 1, 1 ) * y + 2.0 * z * z * gsl_matrix_get( bary, 1, 2 ) + 2.0 * z * gsl_matrix_get( bary, 1, 3 )) * nz  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q1+3,   C * (2.0 * gsl_matrix_get( bary, 1, 2 ) * x * nx - (4.0 * gsl_matrix_get( bary, 1, 0 ) * x + 2.0 * gsl_matrix_get( bary, 1, 1 ) * y + 2.0 * gsl_matrix_get( bary, 1, 2 ) * z + 2.0 * gsl_matrix_get( bary, 1, 3 )) * nz  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q1+4,   C * (gsl_matrix_get( bary, 1, 2 ) * y * y * nx - gsl_matrix_get( bary, 1, 0 ) * y * y * nz  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q1+5,   C * ((2.0 * y * y * gsl_matrix_get( bary, 1, 1 ) + 2.0 * y * gsl_matrix_get( bary, 1, 0 ) * x + (4.0 * gsl_matrix_get( bary, 1, 2 ) * z + 2.0 * gsl_matrix_get( bary, 1, 3 )) * y) * nx - 2.0 * gsl_matrix_get( bary, 1, 0 ) * z * y * nz  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q1+6,   C * (2.0 * gsl_matrix_get( bary, 1, 2 ) * y * nx - 2.0 * gsl_matrix_get( bary, 1, 0 ) * y * nz  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q1+7,   C * ((2.0 * z * gsl_matrix_get( bary, 1, 0 ) * x + 2.0 * z * gsl_matrix_get( bary, 1, 3 ) + 2.0 * z * gsl_matrix_get( bary, 1, 1 ) * y + 3.0 * z * z * gsl_matrix_get( bary, 1, 2 )) * nx - gsl_matrix_get( bary, 1, 0 ) * z * z * nz  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q1+8,   C * ((2.0 * gsl_matrix_get( bary, 1, 0 ) * x + 2.0 * gsl_matrix_get( bary, 1, 3 ) + 2.0 * gsl_matrix_get( bary, 1, 1 ) * y + 4.0 * gsl_matrix_get( bary, 1, 2 ) * z) * nx - 2.0 * gsl_matrix_get( bary, 1, 0 ) * z * nz  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q2+0,   C * (gsl_matrix_get( bary, 2, 2 ) * x * x * nx - ((2.0 * gsl_matrix_get( bary, 2, 1 ) * y + 2.0 * gsl_matrix_get( bary, 2, 3 ) + 2.0 * gsl_matrix_get( bary, 2, 2 ) * z) * x + 3.0 * x * x * gsl_matrix_get( bary, 2, 0 )) * nz  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q2+1,   C * (2.0 * gsl_matrix_get( bary, 2, 2 ) * y * x * nx - (2.0 * y * y * gsl_matrix_get( bary, 2, 1 ) + 4.0 * y * gsl_matrix_get( bary, 2, 0 ) * x + (2.0 * gsl_matrix_get( bary, 2, 3 ) + 2.0 * gsl_matrix_get( bary, 2, 2 ) * z) * y) * nz  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q2+2,   C * ((2.0 * x * x * gsl_matrix_get( bary, 2, 0 ) + (2.0 * gsl_matrix_get( bary, 2, 3 ) + 4.0 * gsl_matrix_get( bary, 2, 2 ) * z + 2.0 * gsl_matrix_get( bary, 2, 1 ) * y) * x) * nx - (2.0 * z * z * gsl_matrix_get( bary, 2, 2 ) + 4.0 * z * gsl_matrix_get( bary, 2, 0 ) * x + 2.0 * z * gsl_matrix_get( bary, 2, 1 ) * y + 2.0 * z * gsl_matrix_get( bary, 2, 3 )) * nz  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q2+3,   C * (2.0 * gsl_matrix_get( bary, 2, 2 ) * x * nx - (2.0 * gsl_matrix_get( bary, 2, 1 ) * y + 4.0 * gsl_matrix_get( bary, 2, 0 ) * x + 2.0 * gsl_matrix_get( bary, 2, 3 ) + 2.0 * gsl_matrix_get( bary, 2, 2 ) * z) * nz  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q2+4,   C * (gsl_matrix_get( bary, 2, 2 ) * y * y * nx - gsl_matrix_get( bary, 2, 0 ) * y * y * nz  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q2+5,   C * ((2.0 * y * y * gsl_matrix_get( bary, 2, 1 ) + 2.0 * y * gsl_matrix_get( bary, 2, 0 ) * x + (4.0 * gsl_matrix_get( bary, 2, 2 ) * z + 2.0 * gsl_matrix_get( bary, 2, 3 )) * y) * nx - 2.0 * gsl_matrix_get( bary, 2, 0 ) * z * y * nz  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q2+6,   C * (2.0 * gsl_matrix_get( bary, 2, 2 ) * y * nx - 2.0 * gsl_matrix_get( bary, 2, 0 ) * y * nz  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q2+7,   C * ((3.0 * z * z * gsl_matrix_get( bary, 2, 2 ) + 2.0 * z * gsl_matrix_get( bary, 2, 3 ) + 2.0 * z * gsl_matrix_get( bary, 2, 0 ) * x + 2.0 * z * gsl_matrix_get( bary, 2, 1 ) * y) * nx - gsl_matrix_get( bary, 2, 0 ) * z * z * nz  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q2+8,   C * ((2.0 * gsl_matrix_get( bary, 2, 3 ) + 4.0 * gsl_matrix_get( bary, 2, 2 ) * z + 2.0 * gsl_matrix_get( bary, 2, 0 ) * x + 2.0 * gsl_matrix_get( bary, 2, 1 ) * y) * nx - 2.0 * gsl_matrix_get( bary, 2, 0 ) * z * nz  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q3+0,   C * (gsl_matrix_get( bary, 3, 2 ) * x * x * nx - (3.0 * x * x * gsl_matrix_get( bary, 3, 0 ) + (2.0 * gsl_matrix_get( bary, 3, 3 ) + 2.0 * gsl_matrix_get( bary, 3, 2 ) * z + 2.0 * gsl_matrix_get( bary, 3, 1 ) * y) * x) * nz  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q3+1,   C * (2.0 * gsl_matrix_get( bary, 3, 2 ) * y * x * nx - (4.0 * y * gsl_matrix_get( bary, 3, 0 ) * x + 2.0 * y * y * gsl_matrix_get( bary, 3, 1 ) + (2.0 * gsl_matrix_get( bary, 3, 2 ) * z + 2.0 * gsl_matrix_get( bary, 3, 3 )) * y) * nz  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q3+2,   C * ((2.0 * x * x * gsl_matrix_get( bary, 3, 0 ) + (2.0 * gsl_matrix_get( bary, 3, 1 ) * y + 2.0 * gsl_matrix_get( bary, 3, 3 ) + 4.0 * gsl_matrix_get( bary, 3, 2 ) * z) * x) * nx - (2.0 * z * z * gsl_matrix_get( bary, 3, 2 ) + 2.0 * z * gsl_matrix_get( bary, 3, 3 ) + 2.0 * z * gsl_matrix_get( bary, 3, 1 ) * y + 4.0 * z * gsl_matrix_get( bary, 3, 0 ) * x) * nz  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q3+3,   C * (2.0 * gsl_matrix_get( bary, 3, 2 ) * x * nx - (2.0 * gsl_matrix_get( bary, 3, 1 ) * y + 4.0 * gsl_matrix_get( bary, 3, 0 ) * x + 2.0 * gsl_matrix_get( bary, 3, 3 ) + 2.0 * gsl_matrix_get( bary, 3, 2 ) * z) * nz  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q3+4,   C * (gsl_matrix_get( bary, 3, 2 ) * y * y * nx - y * y * gsl_matrix_get( bary, 3, 0 ) * nz  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q3+5,   C * ((2.0 * y * gsl_matrix_get( bary, 3, 0 ) * x + 2.0 * y * y * gsl_matrix_get( bary, 3, 1 ) + (4.0 * gsl_matrix_get( bary, 3, 2 ) * z + 2.0 * gsl_matrix_get( bary, 3, 3 )) * y) * nx - 2.0 * z * gsl_matrix_get( bary, 3, 0 ) * y * nz  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q3+6,   C * (2.0 * gsl_matrix_get( bary, 3, 2 ) * y * nx - 2.0 * y * gsl_matrix_get( bary, 3, 0 ) * nz  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q3+7,   C * ((3.0 * z * z * gsl_matrix_get( bary, 3, 2 ) + 2.0 * z * gsl_matrix_get( bary, 3, 1 ) * y + 2.0 * z * gsl_matrix_get( bary, 3, 3 ) + 2.0 * z * gsl_matrix_get( bary, 3, 0 ) * x) * nx - z * z * gsl_matrix_get( bary, 3, 0 ) * nz  )   ) ;
  gsl_matrix_set( M, 4*n+2, 9*q3+8,   C * ((2.0 * gsl_matrix_get( bary, 3, 1 ) * y + 2.0 * gsl_matrix_get( bary, 3, 3 ) + 2.0 * gsl_matrix_get( bary, 3, 0 ) * x + 4.0 * gsl_matrix_get( bary, 3, 2 ) * z) * nx - 2.0 * z * gsl_matrix_get( bary, 3, 0 ) * nz  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q0+0,   C * (-gsl_matrix_get( bary, 0, 1 ) * x * x * nx + (3.0 * x * x * gsl_matrix_get( bary, 0, 0 ) + (2.0 * gsl_matrix_get( bary, 0, 3 ) + 2.0 * gsl_matrix_get( bary, 0, 1 ) * y + 2.0 * gsl_matrix_get( bary, 0, 2 ) * z) * x) * ny  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q0+1,   C * (-((2.0 * gsl_matrix_get( bary, 0, 3 ) + 4.0 * gsl_matrix_get( bary, 0, 1 ) * y + 2.0 * gsl_matrix_get( bary, 0, 2 ) * z) * x + 2.0 * x * x * gsl_matrix_get( bary, 0, 0 )) * nx + ((2.0 * gsl_matrix_get( bary, 0, 2 ) * z + 2.0 * gsl_matrix_get( bary, 0, 3 )) * y + 2.0 * y * y * gsl_matrix_get( bary, 0, 1 ) + 4.0 * y * gsl_matrix_get( bary, 0, 0 ) * x) * ny  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q0+2,   C * ((2.0 * z * gsl_matrix_get( bary, 0, 3 ) + 2.0 * z * gsl_matrix_get( bary, 0, 1 ) * y + 4.0 * z * gsl_matrix_get( bary, 0, 0 ) * x + 2.0 * z * z * gsl_matrix_get( bary, 0, 2 )) * ny - 2.0 * gsl_matrix_get( bary, 0, 1 ) * z * x * nx  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q0+3,   C * ((2.0 * gsl_matrix_get( bary, 0, 2 ) * z + 2.0 * gsl_matrix_get( bary, 0, 1 ) * y + 2.0 * gsl_matrix_get( bary, 0, 3 ) + 4.0 * gsl_matrix_get( bary, 0, 0 ) * x) * ny - 2.0 * gsl_matrix_get( bary, 0, 1 ) * x * nx  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q0+4,   C * (gsl_matrix_get( bary, 0, 0 ) * y * y * ny - ((2.0 * gsl_matrix_get( bary, 0, 2 ) * z + 2.0 * gsl_matrix_get( bary, 0, 3 )) * y + 3.0 * y * y * gsl_matrix_get( bary, 0, 1 ) + 2.0 * y * gsl_matrix_get( bary, 0, 0 ) * x) * nx  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q0+5,   C * (2.0 * gsl_matrix_get( bary, 0, 0 ) * z * y * ny - (2.0 * z * gsl_matrix_get( bary, 0, 3 ) + 2.0 * z * z * gsl_matrix_get( bary, 0, 2 ) + 4.0 * z * gsl_matrix_get( bary, 0, 1 ) * y + 2.0 * z * gsl_matrix_get( bary, 0, 0 ) * x) * nx  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q0+6,   C * (2.0 * gsl_matrix_get( bary, 0, 0 ) * y * ny - (2.0 * gsl_matrix_get( bary, 0, 3 ) + 4.0 * gsl_matrix_get( bary, 0, 1 ) * y + 2.0 * gsl_matrix_get( bary, 0, 2 ) * z + 2.0 * gsl_matrix_get( bary, 0, 0 ) * x) * nx  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q0+7,   C * (gsl_matrix_get( bary, 0, 0 ) * z * z * ny - gsl_matrix_get( bary, 0, 1 ) * z * z * nx  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q0+8,   C * (2.0 * gsl_matrix_get( bary, 0, 0 ) * z * ny - 2.0 * gsl_matrix_get( bary, 0, 1 ) * z * nx  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q1+0,   C * (((2.0 * gsl_matrix_get( bary, 1, 1 ) * y + 2.0 * gsl_matrix_get( bary, 1, 3 ) + 2.0 * gsl_matrix_get( bary, 1, 2 ) * z) * x + 3.0 * x * x * gsl_matrix_get( bary, 1, 0 )) * ny - gsl_matrix_get( bary, 1, 1 ) * x * x * nx  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q1+1,   C * ((4.0 * y * gsl_matrix_get( bary, 1, 0 ) * x + (2.0 * gsl_matrix_get( bary, 1, 2 ) * z + 2.0 * gsl_matrix_get( bary, 1, 3 )) * y + 2.0 * y * y * gsl_matrix_get( bary, 1, 1 )) * ny - ((4.0 * gsl_matrix_get( bary, 1, 1 ) * y + 2.0 * gsl_matrix_get( bary, 1, 2 ) * z + 2.0 * gsl_matrix_get( bary, 1, 3 )) * x + 2.0 * x * x * gsl_matrix_get( bary, 1, 0 )) * nx  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q1+2,   C * ((4.0 * z * gsl_matrix_get( bary, 1, 0 ) * x + 2.0 * z * gsl_matrix_get( bary, 1, 1 ) * y + 2.0 * z * z * gsl_matrix_get( bary, 1, 2 ) + 2.0 * z * gsl_matrix_get( bary, 1, 3 )) * ny - 2.0 * gsl_matrix_get( bary, 1, 1 ) * z * x * nx  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q1+3,   C * ((4.0 * gsl_matrix_get( bary, 1, 0 ) * x + 2.0 * gsl_matrix_get( bary, 1, 1 ) * y + 2.0 * gsl_matrix_get( bary, 1, 2 ) * z + 2.0 * gsl_matrix_get( bary, 1, 3 )) * ny - 2.0 * gsl_matrix_get( bary, 1, 1 ) * x * nx  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q1+4,   C * (gsl_matrix_get( bary, 1, 0 ) * y * y * ny - ((2.0 * gsl_matrix_get( bary, 1, 2 ) * z + 2.0 * gsl_matrix_get( bary, 1, 3 )) * y + 2.0 * y * gsl_matrix_get( bary, 1, 0 ) * x + 3.0 * y * y * gsl_matrix_get( bary, 1, 1 )) * nx  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q1+5,   C * (2.0 * gsl_matrix_get( bary, 1, 0 ) * z * y * ny - (4.0 * z * gsl_matrix_get( bary, 1, 1 ) * y + 2.0 * z * gsl_matrix_get( bary, 1, 3 ) + 2.0 * z * gsl_matrix_get( bary, 1, 0 ) * x + 2.0 * z * z * gsl_matrix_get( bary, 1, 2 )) * nx  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q1+6,   C * (2.0 * gsl_matrix_get( bary, 1, 0 ) * y * ny - (2.0 * gsl_matrix_get( bary, 1, 0 ) * x + 2.0 * gsl_matrix_get( bary, 1, 3 ) + 4.0 * gsl_matrix_get( bary, 1, 1 ) * y + 2.0 * gsl_matrix_get( bary, 1, 2 ) * z) * nx  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q1+7,   C * (gsl_matrix_get( bary, 1, 0 ) * z * z * ny - gsl_matrix_get( bary, 1, 1 ) * z * z * nx  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q1+8,   C * (2.0 * gsl_matrix_get( bary, 1, 0 ) * z * ny - 2.0 * gsl_matrix_get( bary, 1, 1 ) * z * nx  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q2+0,   C * (((2.0 * gsl_matrix_get( bary, 2, 1 ) * y + 2.0 * gsl_matrix_get( bary, 2, 3 ) + 2.0 * gsl_matrix_get( bary, 2, 2 ) * z) * x + 3.0 * x * x * gsl_matrix_get( bary, 2, 0 )) * ny - gsl_matrix_get( bary, 2, 1 ) * x * x * nx  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q2+1,   C * ((2.0 * y * y * gsl_matrix_get( bary, 2, 1 ) + 4.0 * y * gsl_matrix_get( bary, 2, 0 ) * x + (2.0 * gsl_matrix_get( bary, 2, 3 ) + 2.0 * gsl_matrix_get( bary, 2, 2 ) * z) * y) * ny - ((2.0 * gsl_matrix_get( bary, 2, 2 ) * z + 2.0 * gsl_matrix_get( bary, 2, 3 ) + 4.0 * gsl_matrix_get( bary, 2, 1 ) * y) * x + 2.0 * x * x * gsl_matrix_get( bary, 2, 0 )) * nx  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q2+2,   C * ((2.0 * z * z * gsl_matrix_get( bary, 2, 2 ) + 4.0 * z * gsl_matrix_get( bary, 2, 0 ) * x + 2.0 * z * gsl_matrix_get( bary, 2, 1 ) * y + 2.0 * z * gsl_matrix_get( bary, 2, 3 )) * ny - 2.0 * gsl_matrix_get( bary, 2, 1 ) * z * x * nx  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q2+3,   C * ((2.0 * gsl_matrix_get( bary, 2, 1 ) * y + 4.0 * gsl_matrix_get( bary, 2, 0 ) * x + 2.0 * gsl_matrix_get( bary, 2, 3 ) + 2.0 * gsl_matrix_get( bary, 2, 2 ) * z) * ny - 2.0 * gsl_matrix_get( bary, 2, 1 ) * x * nx  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q2+4,   C * (gsl_matrix_get( bary, 2, 0 ) * y * y * ny - ((2.0 * gsl_matrix_get( bary, 2, 3 ) + 2.0 * gsl_matrix_get( bary, 2, 2 ) * z) * y + 3.0 * y * y * gsl_matrix_get( bary, 2, 1 ) + 2.0 * y * gsl_matrix_get( bary, 2, 0 ) * x) * nx  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q2+5,   C * (2.0 * gsl_matrix_get( bary, 2, 0 ) * z * y * ny - (2.0 * z * z * gsl_matrix_get( bary, 2, 2 ) + 4.0 * z * gsl_matrix_get( bary, 2, 1 ) * y + 2.0 * z * gsl_matrix_get( bary, 2, 3 ) + 2.0 * z * gsl_matrix_get( bary, 2, 0 ) * x) * nx  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q2+6,   C * (2.0 * gsl_matrix_get( bary, 2, 0 ) * y * ny - (2.0 * gsl_matrix_get( bary, 2, 0 ) * x + 2.0 * gsl_matrix_get( bary, 2, 3 ) + 4.0 * gsl_matrix_get( bary, 2, 1 ) * y + 2.0 * gsl_matrix_get( bary, 2, 2 ) * z) * nx  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q2+7,   C * (gsl_matrix_get( bary, 2, 0 ) * z * z * ny - gsl_matrix_get( bary, 2, 1 ) * z * z * nx  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q2+8,   C * (2.0 * gsl_matrix_get( bary, 2, 0 ) * z * ny - 2.0 * gsl_matrix_get( bary, 2, 1 ) * z * nx  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q3+0,   C * ((3.0 * x * x * gsl_matrix_get( bary, 3, 0 ) + (2.0 * gsl_matrix_get( bary, 3, 3 ) + 2.0 * gsl_matrix_get( bary, 3, 2 ) * z + 2.0 * gsl_matrix_get( bary, 3, 1 ) * y) * x) * ny - gsl_matrix_get( bary, 3, 1 ) * x * x * nx  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q3+1,   C * ((4.0 * y * gsl_matrix_get( bary, 3, 0 ) * x + 2.0 * y * y * gsl_matrix_get( bary, 3, 1 ) + (2.0 * gsl_matrix_get( bary, 3, 2 ) * z + 2.0 * gsl_matrix_get( bary, 3, 3 )) * y) * ny - (2.0 * x * x * gsl_matrix_get( bary, 3, 0 ) + (4.0 * gsl_matrix_get( bary, 3, 1 ) * y + 2.0 * gsl_matrix_get( bary, 3, 2 ) * z + 2.0 * gsl_matrix_get( bary, 3, 3 )) * x) * nx  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q3+2,   C * ((2.0 * z * z * gsl_matrix_get( bary, 3, 2 ) + 2.0 * z * gsl_matrix_get( bary, 3, 3 ) + 2.0 * z * gsl_matrix_get( bary, 3, 1 ) * y + 4.0 * z * gsl_matrix_get( bary, 3, 0 ) * x) * ny - 2.0 * gsl_matrix_get( bary, 3, 1 ) * z * x * nx  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q3+3,   C * ((2.0 * gsl_matrix_get( bary, 3, 1 ) * y + 4.0 * gsl_matrix_get( bary, 3, 0 ) * x + 2.0 * gsl_matrix_get( bary, 3, 3 ) + 2.0 * gsl_matrix_get( bary, 3, 2 ) * z) * ny - 2.0 * gsl_matrix_get( bary, 3, 1 ) * x * nx  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q3+4,   C * (gsl_matrix_get( bary, 3, 0 ) * y * y * ny - (2.0 * y * gsl_matrix_get( bary, 3, 0 ) * x + 3.0 * y * y * gsl_matrix_get( bary, 3, 1 ) + (2.0 * gsl_matrix_get( bary, 3, 2 ) * z + 2.0 * gsl_matrix_get( bary, 3, 3 )) * y) * nx  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q3+5,   C * (2.0 * gsl_matrix_get( bary, 3, 0 ) * z * y * ny - (2.0 * z * gsl_matrix_get( bary, 3, 3 ) + 2.0 * z * z * gsl_matrix_get( bary, 3, 2 ) + 2.0 * z * gsl_matrix_get( bary, 3, 0 ) * x + 4.0 * z * gsl_matrix_get( bary, 3, 1 ) * y) * nx  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q3+6,   C * (2.0 * gsl_matrix_get( bary, 3, 0 ) * y * ny - (2.0 * gsl_matrix_get( bary, 3, 0 ) * x + 2.0 * gsl_matrix_get( bary, 3, 3 ) + 4.0 * gsl_matrix_get( bary, 3, 1 ) * y + 2.0 * gsl_matrix_get( bary, 3, 2 ) * z) * nx  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q3+7,   C * (gsl_matrix_get( bary, 3, 0 ) * z * z * ny - z * z * gsl_matrix_get( bary, 3, 1 ) * nx  )   ) ;
  gsl_matrix_set( M, 4*n+3, 9*q3+8,   C * (2.0 * gsl_matrix_get( bary, 3, 0 ) * z * ny - 2.0 * gsl_matrix_get( bary, 3, 1 ) * z * nx  )   ) ;

  gsl_vector_set( U, 4*n+0,   gsl_matrix_get( bary, 3, 3 ) + gsl_matrix_get( bary, 0, 3 ) + gsl_matrix_get( bary, 1, 3 ) + gsl_matrix_get( bary, 2, 3 ) + gsl_matrix_get( bary, 0, 0 ) * x + gsl_matrix_get( bary, 0, 1 ) * y + gsl_matrix_get( bary, 0, 2 ) * z + gsl_matrix_get( bary, 1, 0 ) * x + gsl_matrix_get( bary, 1, 1 ) * y + gsl_matrix_get( bary, 1, 2 ) * z + gsl_matrix_get( bary, 2, 0 ) * x + gsl_matrix_get( bary, 2, 1 ) * y + gsl_matrix_get( bary, 2, 2 ) * z + gsl_matrix_get( bary, 3, 0 ) * x + gsl_matrix_get( bary, 3, 1 ) * y + gsl_matrix_get( bary, 3, 2 ) * z  ) ;
  gsl_vector_set( U, 4*n+1,   C * (-(-gsl_matrix_get( bary, 3, 1 ) - gsl_matrix_get( bary, 0, 1 ) - gsl_matrix_get( bary, 1, 1 ) - gsl_matrix_get( bary, 2, 1 )) * nz + (-gsl_matrix_get( bary, 1, 2 ) - gsl_matrix_get( bary, 2, 2 ) - gsl_matrix_get( bary, 0, 2 ) - gsl_matrix_get( bary, 3, 2 )) * ny  )   ) ;
  gsl_vector_set( U, 4*n+2,   C * (-(-gsl_matrix_get( bary, 1, 2 ) - gsl_matrix_get( bary, 2, 2 ) - gsl_matrix_get( bary, 0, 2 ) - gsl_matrix_get( bary, 3, 2 )) * nx + (-gsl_matrix_get( bary, 1, 0 ) - gsl_matrix_get( bary, 2, 0 ) - gsl_matrix_get( bary, 0, 0 ) - gsl_matrix_get( bary, 3, 0 )) * nz  )   ) ;
  gsl_vector_set( U, 4*n+3,   C * (-(-gsl_matrix_get( bary, 1, 0 ) - gsl_matrix_get( bary, 2, 0 ) - gsl_matrix_get( bary, 0, 0 ) - gsl_matrix_get( bary, 3, 0 )) * ny + (-gsl_matrix_get( bary, 3, 1 ) - gsl_matrix_get( bary, 0, 1 ) - gsl_matrix_get( bary, 1, 1 ) - gsl_matrix_get( bary, 2, 1 )) * nx  )   ) ;

  return true ;
}



// -------------------------------------------------------- //
// create matrix of point and normal constraints
int cubic_coeff_constraint( const gsl_matrix *bary, gsl_matrix *M, gsl_vector *U, int n /* first line*/, int q0, int q1, int q2, int q3 )
// -------------------------------------------------------- //
{
  gsl_matrix_set( M, n+0, 9*q0+0,   D * gsl_matrix_get( bary, 0,0  )  ) ;
  gsl_matrix_set( M, n+0, 9*q1+0,   D * gsl_matrix_get( bary, 1,0  )  ) ;
  gsl_matrix_set( M, n+0, 9*q2+0,   D * gsl_matrix_get( bary, 2,0  )  ) ;
  gsl_matrix_set( M, n+0, 9*q3+0,   D * gsl_matrix_get( bary, 3,0  )  ) ;
  gsl_matrix_set( M, n+1, 9*q0+0,   D * gsl_matrix_get( bary, 0,1  )  ) ;
  gsl_matrix_set( M, n+1, 9*q0+1,   D * 2 * gsl_matrix_get( bary, 0,0  )  ) ;
  gsl_matrix_set( M, n+1, 9*q1+0,   D * gsl_matrix_get( bary, 1,1  )  ) ;
  gsl_matrix_set( M, n+1, 9*q1+1,   D * 2 * gsl_matrix_get( bary, 1,0  )  ) ;
  gsl_matrix_set( M, n+1, 9*q2+0,   D * gsl_matrix_get( bary, 2,1  )  ) ;
  gsl_matrix_set( M, n+1, 9*q2+1,   D * 2 * gsl_matrix_get( bary, 2,0  )  ) ;
  gsl_matrix_set( M, n+1, 9*q3+0,   D * gsl_matrix_get( bary, 3,1  )  ) ;
  gsl_matrix_set( M, n+1, 9*q3+1,   D * 2 * gsl_matrix_get( bary, 3,0  )  ) ;
  gsl_matrix_set( M, n+2, 9*q0+0,   D * gsl_matrix_get( bary, 0,2  )  ) ;
  gsl_matrix_set( M, n+2, 9*q0+2,   D * 2 * gsl_matrix_get( bary, 0,0  )  ) ;
  gsl_matrix_set( M, n+2, 9*q1+0,   D * gsl_matrix_get( bary, 1,2  )  ) ;
  gsl_matrix_set( M, n+2, 9*q1+2,   D * 2 * gsl_matrix_get( bary, 1,0  )  ) ;
  gsl_matrix_set( M, n+2, 9*q2+0,   D * gsl_matrix_get( bary, 2,2  )  ) ;
  gsl_matrix_set( M, n+2, 9*q2+2,   D * 2 * gsl_matrix_get( bary, 2,0  )  ) ;
  gsl_matrix_set( M, n+2, 9*q3+0,   D * gsl_matrix_get( bary, 3,2  )  ) ;
  gsl_matrix_set( M, n+2, 9*q3+0,   D * 2 * gsl_matrix_get( bary, 3,0  )  ) ;
  gsl_matrix_set( M, n+3, 9*q0+1,   D * 2 * gsl_matrix_get( bary, 0,1  )  ) ;
  gsl_matrix_set( M, n+3, 9*q0+4,   D * gsl_matrix_get( bary, 0,0  )  ) ;
  gsl_matrix_set( M, n+3, 9*q1+1,   D * 2 * gsl_matrix_get( bary, 1,1  )  ) ;
  gsl_matrix_set( M, n+3, 9*q1+4,   D * gsl_matrix_get( bary, 1,0  )  ) ;
  gsl_matrix_set( M, n+3, 9*q2+1,   D * 2 * gsl_matrix_get( bary, 2,1  )  ) ;
  gsl_matrix_set( M, n+3, 9*q2+4,   D * gsl_matrix_get( bary, 2,0  )  ) ;
  gsl_matrix_set( M, n+3, 9*q3+1,   D * 2 * gsl_matrix_get( bary, 3,1  )  ) ;
  gsl_matrix_set( M, n+3, 9*q3+4,   D * gsl_matrix_get( bary, 3,0  )  ) ;
  gsl_matrix_set( M, n+4, 9*q0+1,   D * 2 * gsl_matrix_get( bary, 0,2  )  ) ;
  gsl_matrix_set( M, n+4, 9*q0+2,   D * 2 * gsl_matrix_get( bary, 0,1  )  ) ;
  gsl_matrix_set( M, n+4, 9*q0+5,   D * 2 * gsl_matrix_get( bary, 0,0  )  ) ;
  gsl_matrix_set( M, n+4, 9*q1+1,   D * 2 * gsl_matrix_get( bary, 1,2  )  ) ;
  gsl_matrix_set( M, n+4, 9*q1+2,   D * 2 * gsl_matrix_get( bary, 1,1  )  ) ;
  gsl_matrix_set( M, n+4, 9*q1+5,   D * 2 * gsl_matrix_get( bary, 1,0  )  ) ;
  gsl_matrix_set( M, n+4, 9*q2+1,   D * 2 * gsl_matrix_get( bary, 2,2  )  ) ;
  gsl_matrix_set( M, n+4, 9*q2+2,   D * 2 * gsl_matrix_get( bary, 2,1  )  ) ;
  gsl_matrix_set( M, n+4, 9*q2+5,   D * 2 * gsl_matrix_get( bary, 2,0  )  ) ;
  gsl_matrix_set( M, n+4, 9*q3+1,   D * 2 * gsl_matrix_get( bary, 3,2  )  ) ;
  gsl_matrix_set( M, n+4, 9*q3+2,   D * 2 * gsl_matrix_get( bary, 3,1  )  ) ;
  gsl_matrix_set( M, n+4, 9*q3+5,   D * 2 * gsl_matrix_get( bary, 3,0  )  ) ;
  gsl_matrix_set( M, n+5, 9*q0+2,   D * 2 * gsl_matrix_get( bary, 0,2  )  ) ;
  gsl_matrix_set( M, n+5, 9*q0+7,   D * gsl_matrix_get( bary, 0,0  )  ) ;
  gsl_matrix_set( M, n+5, 9*q1+2,   D * 2 * gsl_matrix_get( bary, 1,2  )  ) ;
  gsl_matrix_set( M, n+5, 9*q1+7,   D * gsl_matrix_get( bary, 1,0  )  ) ;
  gsl_matrix_set( M, n+5, 9*q2+2,   D * 2 * gsl_matrix_get( bary, 2,2  )  ) ;
  gsl_matrix_set( M, n+5, 9*q2+7,   D * gsl_matrix_get( bary, 2,0  )  ) ;
  gsl_matrix_set( M, n+5, 9*q3+2,   D * 2 * gsl_matrix_get( bary, 3,2  )  ) ;
  gsl_matrix_set( M, n+5, 9*q3+7,   D * gsl_matrix_get( bary, 3,0  )  ) ;
  gsl_matrix_set( M, n+6, 9*q0+4,   D * gsl_matrix_get( bary, 0,1  )  ) ;
  gsl_matrix_set( M, n+6, 9*q1+4,   D * gsl_matrix_get( bary, 1,1  )  ) ;
  gsl_matrix_set( M, n+6, 9*q2+4,   D * gsl_matrix_get( bary, 2,1  )  ) ;
  gsl_matrix_set( M, n+6, 9*q3+4,   D * gsl_matrix_get( bary, 3,1  )  ) ;
  gsl_matrix_set( M, n+7, 9*q0+4,   D * gsl_matrix_get( bary, 0,2  )  ) ;
  gsl_matrix_set( M, n+7, 9*q0+5,   D * 2 * gsl_matrix_get( bary, 0,1  )  ) ;
  gsl_matrix_set( M, n+7, 9*q1+4,   D * gsl_matrix_get( bary, 1,2  )  ) ;
  gsl_matrix_set( M, n+7, 9*q1+5,   D * 2 * gsl_matrix_get( bary, 1,1  )  ) ;
  gsl_matrix_set( M, n+7, 9*q2+4,   D * gsl_matrix_get( bary, 2,2  )  ) ;
  gsl_matrix_set( M, n+7, 9*q2+5,   D * 2 * gsl_matrix_get( bary, 2,1  )  ) ;
  gsl_matrix_set( M, n+7, 9*q3+4,   D * gsl_matrix_get( bary, 3,2  )  ) ;
  gsl_matrix_set( M, n+7, 9*q3+5,   D * 2 * gsl_matrix_get( bary, 3,1  )  ) ;
  gsl_matrix_set( M, n+8, 9*q0+5,   D * 2 * gsl_matrix_get( bary, 0,2  )  ) ;
  gsl_matrix_set( M, n+8, 9*q0+7,   D * gsl_matrix_get( bary, 0,1  )  ) ;
  gsl_matrix_set( M, n+8, 9*q1+5,   D * 2 * gsl_matrix_get( bary, 1,2  )  ) ;
  gsl_matrix_set( M, n+8, 9*q1+6,   D * gsl_matrix_get( bary, 1,1  )  ) ;
  gsl_matrix_set( M, n+8, 9*q2+5,   D * 2 * gsl_matrix_get( bary, 2,2  )  ) ;
  gsl_matrix_set( M, n+8, 9*q2+7,   D * gsl_matrix_get( bary, 2,1  )  ) ;
  gsl_matrix_set( M, n+8, 9*q3+5,   D * 2 * gsl_matrix_get( bary, 3,2  )  ) ;
  gsl_matrix_set( M, n+8, 9*q3+7,   D * gsl_matrix_get( bary, 3,1  )  ) ;
  gsl_matrix_set( M, n+9, 9*q0+7,   D * gsl_matrix_get( bary, 0,2  )  ) ;
  gsl_matrix_set( M, n+9, 9*q1+7,   D * gsl_matrix_get( bary, 1,2  )  ) ;
  gsl_matrix_set( M, n+9, 9*q2+7,   D * gsl_matrix_get( bary, 2,2  )  ) ;
  gsl_matrix_set( M, n+9, 9*q3+7,   D * gsl_matrix_get( bary, 3,2  )  ) ;

  return true ;
}



// -------------------------------------------------------- //
// create matrix of point constraints
int build_system( const gsl_matrix *bary, gsl_matrix *M, gsl_vector *U )
// -------------------------------------------------------- //
{
  if( !M || !U ) return false ;

  if( use_normals )
  {
    for( int n = 0 ; n < N ; ++n )
      single_point_normal_constraint(bary, M, U, n, pointset[n][0], pointset[n][1], pointset[n][2],  normals[n][0], normals[n][1], normals[n][2], 0, 1, 2, 3 ) ;

    if( debug )
    {
      //      print_matrix( "M", 4*N, 36, M ) ;
      //      print_vector( "U", 4*N, U ) ;

      for (int n = 0; n < N; ++n)
      {
        real s  = -gsl_vector_get(U, 4*n  ) ;
        real dx = -gsl_vector_get(U, 4*n+1) ;
        real dy = -gsl_vector_get(U, 4*n+2) ;
        real dz = -gsl_vector_get(U, 4*n+3) ;
        for (int j = 0; j < 36; ++j)
        {
          s  += gsl_matrix_get(M, 4*n  , j)*Q_ref[j];
          dx += gsl_matrix_get(M, 4*n+1, j)*Q_ref[j];
          dy += gsl_matrix_get(M, 4*n+2, j)*Q_ref[j];
          dz += gsl_matrix_get(M, 4*n+3, j)*Q_ref[j];
        }
        printf("M*Q-U [%d] = %.2g, (%.2g, %.2g, %.2g) \n", n, s, dx, dy, dz);
      }
      printf("\n");
    }
  }
  else
  {
    for( int n = 0 ; n < N ; ++n )
      single_point_constraint(bary, M, U, n, pointset[n][0], pointset[n][1], pointset[n][2], 0, 1, 2, 3 ) ;

    if( debug )
    {
      //      print_matrix( "M", N, 36, M ) ;
      //      print_vector( "U", N, U ) ;

      for (int n = 0; n < N; ++n)
      {
        real s  = -gsl_vector_get(U, n) ;
        for (int j = 0; j < 36; ++j)
        {
          s  += gsl_matrix_get(M, n, j)*Q_ref[j];
        }
        printf("M*Q-U [%d] = %.2g \n", n, s);
      }
      printf("\n");
    }
  }

  if ( reduce_higher_coefficients )
  {
    if( use_normals )
      cubic_coeff_constraint(bary, M, U, 4*N, 0,1,2,3 ) ;
    else
      cubic_coeff_constraint(bary, M, U,  N , 0,1,2,3 ) ;
  }

  return true ;
}
// -------------------------------------------------------- //pq eu



// -------------------------------------------------------- //
// solve min || Mq - b ||
int minimize( const gsl_matrix *M, const gsl_vector *U, gsl_vector *X )
// -------------------------------------------------------- //
{
  gsl_multifit_linear_workspace * work ;
  if( use_normals )
  {
    if ( reduce_higher_coefficients )
      work = gsl_multifit_linear_alloc(4*N+10,36);
    else
      work = gsl_multifit_linear_alloc(4*N,36);
  }
  else
  {
    if ( reduce_higher_coefficients )
      work = gsl_multifit_linear_alloc(N+10,36);
    else
      work = gsl_multifit_linear_alloc(N,36);
  }

  double chisq ;
  gsl_matrix *cov = gsl_matrix_alloc(36,36);

  if(use_normals)
  {
      size_t rank ;
      gsl_multifit_linear_svd (M, U, 0.1, &rank, X, cov, &chisq, work) ;
  }else
  {
      gsl_multifit_linear( M, U, X, cov, &chisq, work );
  }


  gsl_multifit_linear_free( work ) ;

  if(debug) eigen(cov);
  gsl_matrix_free(cov);

  return true ;
}
// -------------------------------------------------------- //



// -------------------------------------------------------- //
// main
int fit_points()
// -------------------------------------------------------- //
{
  // compute barycentric coordinates
  gsl_matrix *bary = gsl_matrix_alloc(4, 4);
  compute_bary_matrix( bary ) ;

  // -------------------------------------------------------- //
  // create matrix of point constraints

  gsl_matrix *M ;
  gsl_vector *U ;
  if( use_normals )
  {
    if ( reduce_higher_coefficients )
    {
      M = gsl_matrix_calloc(4*N+10, 36);
      U = gsl_vector_calloc(4*N+10);
    }
    else
    {
      M = gsl_matrix_calloc(4*N, 36);
      U = gsl_vector_calloc(4*N);
    }
  }
  else
  {
    if ( reduce_higher_coefficients )
    {
      M = gsl_matrix_calloc(N+10, 36);
      U = gsl_vector_calloc(N+10);
    }
    else
    {
      M = gsl_matrix_calloc(N, 36);
      U = gsl_vector_calloc(N);
    }
  }

  build_system( bary, M, U );
  gsl_matrix_free(bary);


  // -------------------------------------------------------- //
  // solve min || Mq  - U ||

  gsl_vector *X = gsl_vector_alloc (36);
  minimize( M, U, X ) ;
  gsl_matrix_free(M);
  gsl_vector_free(U);



  // -------------------------------------------------------- //
  // get the solution
  for( int i = 0 ; i < 4 ; ++i )
  {
    if( debug ) printf( "\nconst mat4 Q%d = mat4(", i ) ;
    for( int j = 0 ; j < 4 ; ++j )
    {
      for( int k = 0 ; k < 4 ; ++k )
      {
        int l = quad_idx[j][k] ;
        int m = 9*i+l ;
        if( l < 0 ) Q[i][j][k] = -1.0 ;
        else Q[i][j][k] = gsl_vector_get( X, m ) ;

        if( debug ) printf( "%f, ", Q[i][j][k] ) ;
      }
    }
    if( debug ) printf( ");\n" ) ;
  }
  gsl_vector_free (X);

  return 0;
}

void allocVectors()
{
    pointset = new real*[N];
    normals  = new real*[N];

    for(int i = 0; i < N; ++i)
    {
        pointset[i] = new real[4];
        normals [i] = new real[3];
    }
}

void freeVectors()
{
    for(int i = 0; i < N; ++i)
    {
        delete [] pointset[i];
        delete [] normals [i];
    }
    delete [] pointset;
    delete [] normals;
}

// -------------------------------------------------------- //


Fitting::Fitting(QObject *parent) :
    QObject(parent)
{
}

void Fitting::tetraQuadFitting(QVector<QVector3D> points,  TetraQuad* tet)
{
    debug = false;
    use_normals = false;

    QVector<QVector4D> Tetra = tet->getVertices();
    for(int i = 0; i < 4; ++i)
    {
        tetra[i][0] = Tetra[i].x();
        tetra[i][1] = Tetra[i].y();
        tetra[i][2] = Tetra[i].z();
        tetra[i][3] = Tetra[i].w();
    }

    N = points.size();
    allocVectors();
    for(int i = 0; i < N; ++i)
    {
        pointset[i][0] = points[i].x();
        pointset[i][1] = points[i].y();
        pointset[i][2] = points[i].z();
        pointset[i][3] = 1.0;
    }

    fit_points();

    freeVectors();

    for(int i = 0; i < 4; ++i)
    {
        QMatrix4x4 tQ;
        for(int j = 0; j < 4; ++j)
            for(int k = 0; k < 4; ++k)
                tQ(j,k) = Q[i][j][k];

        tet->setQuadric(tQ,i);
    }
}

void Fitting::tetraQuadFitting(const QVector<QPair<QVector3D, QVector3D> >& p_n,  TetraQuad* tet)
{
    debug = false;
    use_normals = true;

    QVector<QVector4D> Tetra = tet->getVertices();
    for(int i = 0; i < 4; ++i)
    {
        tetra[i][0] = Tetra[i].x();
        tetra[i][1] = Tetra[i].y();
        tetra[i][2] = Tetra[i].z();
        tetra[i][3] = Tetra[i].w();
    }

    N = p_n.size();
    allocVectors();
    for(int i = 0; i < N; ++i)
    {
        pointset[i][0] = p_n[i].first.x();
        pointset[i][1] = p_n[i].first.y();
        pointset[i][2] = p_n[i].first.z();
        pointset[i][3] = 1.0;

        normals[i][0] = p_n[i].second.x();
        normals[i][1] = p_n[i].second.y();
        normals[i][2] = p_n[i].second.z();
    }

    fit_points();
    freeVectors();

    for(int i = 0; i < 4; ++i)
    {
        QMatrix4x4 tQ;
        for(int j = 0; j < 4; ++j)
            for(int k = 0; k < 4; ++k)
                tQ(j,k) = Q[i][j][k];

        tet->setQuadric(tQ,i);
    }
}
